#include "DiscHandling.h"
#include "../Robotmap.h"
#include "../Commands/DiscPickupControl.h"

DiscHandling::DiscHandling() :
	Subsystem("DiscHandling"), conveyorVictor(CONVEYOR_VICTOR),
			conveyorPin(SOLENOID_MODULE, CONVEYOR_PIN_SOLENOID),
			breachProxSensor(DIGITAL_MODULE, BREACH_PROX_SENSOR),
			topProxSensor(DIGITAL_MODULE, TOP_PROX_SENSOR),
			middleProxSensor(DIGITAL_MODULE, MIDDLE_PROX_SENSOR),
			bottomProxSensor(DIGITAL_MODULE, BOTTOM_PROX_SENSOR) {
	proxUpdater = new Notifier(TimerEventHandler(DiscHandling::UpdateSensors),
			(void*) this);

	proxUpdater->StartPeriodic(0.02);
	extendPin = new Notifier(TimerEventHandler(DiscHandling::ExtendPin),
			(void*) this);
	retractPin = new Notifier(TimerEventHandler(DiscHandling::RetractPin),
			(void*) this);
	startConveyor = new Notifier(
			TimerEventHandler(DiscHandling::StartConveyor), (void*) this);
	stopConveyor = new Notifier(TimerEventHandler(DiscHandling::StopConveyor),
			(void*) this);
	sensors[0] = &bottomProxSensor;
	sensors[1] = &middleProxSensor;
	sensors[2] = &topProxSensor;
	sensors[3] = &breachProxSensor;
	AUTO = false;
}

void DiscHandling::InitDefaultCommand() {
	// Set the default command for a subsystem here.
	//SetDefaultCommand(new MySpecialCommand());
	SetDefaultCommand(new DiscPickupControl());
}

void DiscHandling::UpdateSensors(void * handle) {
	DiscHandling* s = (DiscHandling*) handle;
	for (int i = 0; i < 4; ++i) {
		s->sensorCount[i] += s->sensors[i]->Get() ? 1 : -1;
		if (s->sensorCount[i] > sensorFilterRange) {
			s->sensorCount[i] = sensorFilterRange;
		} else if (s->sensorCount[i] < 0) {
			s->sensorCount[i] = 0;
		}
		s->sensorValue[i] = s->sensorCount[i] > sensorFilterHalf;
	}
}

void DiscHandling::ExtendPin(void* handle) {
	DiscHandling* s = (DiscHandling*) handle;
	s->ControlPinState(true);
}

void DiscHandling::RetractPin(void* handle) {
	DiscHandling* s = (DiscHandling*) handle;
	s->ControlPinState(false);
}

void DiscHandling::PulsePin(void* handle) {
	DiscHandling* s = (DiscHandling*) handle;
	s->extendPin->StartSingle(0.0);
	s->retractPin->StartSingle(0.4);
}
bool DiscHandling::GetSensor(SensorID sensor) {
	if (sensor < 0 || sensor > 3) {
		return false;
		printf("Invalid range of GetSensor: %d", sensor);
	}
	return (sensorValue[sensor]);
}

void DiscHandling::StopConveyor(void* dH) {
	DiscHandling* discHandle = (DiscHandling*) dH;
	printf("Stop Conveyor Notifier");
	discHandle->ControlConveyorState(kConveyorOff);
}
void DiscHandling::StartConveyor(void* dH) {
	DiscHandling* discHandle = (DiscHandling*) dH;
	printf("Starting conveyor");
	discHandle->ControlConveyorState(kConveyorUp);
}

void DiscHandling::ControlConveyorState(ConveyorDirection Direction) {
	//printf("Breach::%d ", breachProxSensor.Get());
	//printf("Top::%d ", topProxSensor.Get());
	//printf("Middle::%d ", middleProxSensor.Get());
	//printf("Bottom::%d \n", bottomProxSensor.Get());

	if (Direction == kConveyorUp) {
		printf("Set conveyor to go UP\n");
		conveyorVictor.Set(0.75);
	} else if (Direction == kConveyorDown) {
		conveyorVictor.Set(-1.0);
		printf("Reverse");
	} else if (Direction == kConveyorOff) {
		printf("Conveyor OFF");
		conveyorVictor.Set(0.0);
	}
}

void DiscHandling::ControlPinState(bool IsOut) {
	conveyorPin.Set(IsOut);
}
int DiscHandling::GetDiscNumber() {
	return (sensorValue[0] + sensorValue[1] + sensorValue[2] + sensorValue[3]);
}

bool DiscHandling::GetIsConveyorUp() {
	if (CommandBase::discHandling->conveyorVictor.Get() >= 0.0) {
		return true;
	} else {
		return false;
	}
}


// Put methods for controlling this subsystem
// here. Call these from Commands.
